
#include <glog/logging.h>

#include "publisher/publisher.h"

// #include "pangolin_viewer/viewer.h"

#include "read_data.h"

using namespace cartographer;
// using namespace pangolin_viewer;

static bool use_pangolin = true;

std::string data_path = "/home/zl/Public/kaist_data/urban_data/";

int main(int argc, char** argv) {

    std::shared_ptr<Publisher> publisher_ptr = std::make_shared<Publisher>();
    if(use_pangolin) {
        // pangolin_viewer::viewer* viewer_ptr = new pangolin_viewer::viewer(publisher_ptr);
        // std::thread* viewer_thread_ptr = new thread(&viewer::run, viewer_ptr);      
    }

    double last_left = 0;
    double last_right = 0;
    double last_wheel_time = 0;
    bool used_simple_point = true;

    Options options_;
    Eigen::Matrix3d Rwo = Eigen::Matrix3d::Identity();
    Eigen::Vector3d pwo = Eigen::Vector3d::Zero();

    std::string data_stamp_path = data_path + "sensor_data/data_stamp.csv";
    std::ifstream fin(data_stamp_path);
    if (!fin) {
        return -1;
    }
    
    std::vector<std::string> line_data_vec;
    line_data_vec.reserve(3);
    std::string line_str, value_str;

    std::unordered_map<std::string, std::string> time_encoder_map;
    std::unordered_map<std::string, std::string> time_imu_map;
    std::unordered_map<std::string, std::string> time_gps_map; 
    std::unordered_map<std::string, std::string> time_vrs_gps_map;
    std::unordered_map<std::string, std::string> time_sick_middle_map;
    std::unordered_map<std::string, std::string> time_velodyne_left_map;

    read_data* read_input_data = new read_data(data_path);
    if(!read_input_data->read_only_kaist_data(time_encoder_map, time_imu_map, time_gps_map, time_vrs_gps_map, time_sick_middle_map, time_velodyne_left_map)) {
        std::cout << "[main]: Failed to find data " << std::endl;
        return -1;
    }
    static int first_frame = 0;
    double first_timestamp = read_input_data->get_origin_timestamp();

    // init options
    NodeOptions node_options;
    node_options.trajectory_id = 0;

    TrajectoryOptions trajectory_options;

    Node node(node_options, read_input_data->get_ext(), publisher_ptr);
    node.StartTrajectoryWithDefaultTopics(trajectory_options);

    // start add data
    while (std::getline(fin, line_str)) {
        line_data_vec.clear();
        std::stringstream ss(line_str);
        while (std::getline(ss, value_str, ',')) { line_data_vec.push_back(value_str); }
        const double kToSecond = 1e-9;
        if(line_data_vec.empty()) {
            continue;
        }
        const std::string& time_str = line_data_vec[0];

        const double timestamp = std::stod(time_str) * kToSecond - first_timestamp;

        const std::string& sensor_type = line_data_vec[1];
        if (sensor_type == "imu") {
            if (time_imu_map.find(time_str) == time_imu_map.end()) {
                std::cout << "[main]: Failed to find imu data at time: " << time_str << std::endl;
                continue;
            }
            const std::string& imu_str = time_imu_map.at(time_str);
            // std::cout << "imu_str : " << imu_str << std::endl;

            if(imu_str.size() < 17) {
                continue;
            }   
            std::stringstream imu_ss(imu_str);
            line_data_vec.clear();
            while (std::getline(imu_ss, value_str, ',')) { line_data_vec.push_back(value_str); }
            if(line_data_vec.empty()) {
                continue;
            }
            double time = timestamp;
            // std::cout << "line_data_vec : " << line_data_vec.size();
            double gx = std::stod(line_data_vec[8]);
            double gy = std::stod(line_data_vec[9]);
            double gz = std::stod(line_data_vec[10]);
            double ax = std::stod(line_data_vec[11]);
            double ay = std::stod(line_data_vec[12]);
            double az = std::stod(line_data_vec[13]);  
            // todo : add data
            sensor::ImuData imu_data;
            imu_data.time = common::FromSecond(time);
            imu_data.linear_acceleration = Eigen::Vector3d(ax, ay, az);
            imu_data.angular_velocity = Eigen::Vector3d(gx, gy, gz);

            // node.HandleImuMessage(node_options.trajectory_id, sensor_type, imu_data);

        } else if (sensor_type == "encoder") {
            if (time_encoder_map.find(time_str) == time_encoder_map.end()) {
                std::cout << "[main]: Failed to find encoder data at time: " << time_str << std::endl;
                continue;
            }

            const std::string& encoder_str = time_encoder_map.at(time_str);
            std::stringstream enc_ss(encoder_str);
            line_data_vec.clear();
            while (std::getline(enc_ss, value_str, ',')) { line_data_vec.push_back(value_str); }
            if(line_data_vec.empty()) {
                continue;
            }
            const double left_enc_cnt = std::stod(line_data_vec[1]);
            const double right_enc_cnt = std::stod(line_data_vec[2]);
            
            static bool is_init = false;
            static double init_left = 0;
            static double init_right = 0;
            if(!is_init) {
                init_left = left_enc_cnt;
                init_right = right_enc_cnt;
                last_left = left_enc_cnt;
                last_right = right_enc_cnt;
                last_wheel_time = timestamp;
                is_init = true;
                continue;
            }
            // std::cout << "init_left : " << init_left;
            // std::cout << "init_right : " << init_right;
            // std::cout << "left_enc_cnt : " << left_enc_cnt;
            // std::cout << "right_enc_cnt : " << right_enc_cnt;

            double time = timestamp;
            double delta_left_pulse = left_enc_cnt - last_left;
            double delta_right_pulse = right_enc_cnt - last_right;

            // double wl = left_enc_cnt - init_left - last_left;
            // double wr = right_enc_cnt - init_right - last_right;

            options_.odom_span_ = time - last_wheel_time;
            if(options_.odom_span_ > 1.0 || options_.odom_span_ < 1.0e-5) {
                options_.odom_span_ = 0.01;
            }  

            double velo_l = options_.left_wheel_radius_ * delta_left_pulse / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
            double velo_r = options_.right_wheel_radius_ * (delta_right_pulse) / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
            // velocity
            double average_vel = 0.5 * (velo_l + velo_r);
            // double average_omega = (velo_r - velo_l) / options_.wheel_base_; 

            const double left_dist = delta_left_pulse * options_.kl_;
            const double right_dist = delta_right_pulse * options_.kr_;

            const double delta_yaw = (right_dist - left_dist) / options_.wheel_base_;
            const double delta_dist = (right_dist + left_dist) * 0.5;
            // Mean.
            const Eigen::Matrix3d delta_R = Eigen::AngleAxisd(delta_yaw, Eigen::Vector3d::UnitZ()).toRotationMatrix();
            const Eigen::Vector3d delta_p = Eigen::Vector3d(delta_dist, 0., 0.);    
            
            // todo : add wheel data 
            pwo = pwo + Rwo * delta_p;
            Rwo = Rwo * delta_R;

            sensor::OdometryData odometry_data;
            odometry_data.time = common::FromSecond(time);
            odometry_data.pose = transform::Rigid3d(pwo, Eigen::Quaterniond(Rwo));

            // node.HandleOdometryMessage(node_options.trajectory_id, sensor_type, odometry_data);

            Eigen::Vector2d p = pwo.head<2>();
            double theta = std::atan2(Rwo(1, 0), Rwo(0, 0));
            publisher_ptr->PublishOdometryPose(Eigen::Vector3d(p[0], p[1], theta));
            
            // std::cout << "average_vel ========= : " << average_vel << std::endl;
            // std::cout << "theta ========= : " << theta << std::endl;
            LOG(INFO) << "p ========= : " << p.transpose() << std::endl;

            last_left = left_enc_cnt;
            last_right = right_enc_cnt;
            last_wheel_time = time;
        }else if (sensor_type == "gps") {
            if (time_gps_map.find(time_str) == time_gps_map.end()) {
                std::cout << "[main]: Failed to find gps data at time: " << time_str << std::endl;
                continue;
            }

            const std::string& gps_str = time_gps_map.at(time_str);

            std::stringstream gps_ss(gps_str);
            line_data_vec.clear();
            while (std::getline(gps_ss, value_str, ',')) { line_data_vec.push_back(value_str); }
            if(line_data_vec.empty()) {
                continue;
            }
            double time = timestamp;
            const double lat = std::stod(line_data_vec[1]);
            const double lon = std::stod(line_data_vec[2]);
            const double alt = std::stod(line_data_vec[3]);

            //Eigen::Matrix3d cov;
            //for (size_t i = 0; i < 9; ++i) {
            //    cov.data()[i] = std::stod(line_data_vec[4+i]);
            //}

            double latitude_std = std::stod(line_data_vec[4]);
            double longitude_std = std::stod(line_data_vec[5]);
            double altitude_std = std::stod(line_data_vec[6]);

            double heading = 0.0;
            bool heading_valid = true;
            
            // gnss_proc_(GNSS(time, 4, Vec3d(lat, lon, alt), heading, heading_valid, Vec3d(latitude_std, longitude_std, altitude_std)));
        
        } else if (sensor_type == "vrs") {
            if (time_vrs_gps_map.find(time_str) == time_vrs_gps_map.end()) {
                std::cout << "[main]: Failed to find gps data at time: " << time_str << std::endl;
                continue;
            }

            const std::string& gps_str = time_vrs_gps_map.at(time_str);
            std::stringstream gps_ss(gps_str);
            line_data_vec.clear();
            while (std::getline(gps_ss, value_str, ',')) { line_data_vec.push_back(value_str); }
            if(line_data_vec.empty()) {
                continue;
            }
            double time = timestamp;

            const double lat = std::stod(line_data_vec[1]);
            const double lon = std::stod(line_data_vec[2]);
            const double x = std::stod(line_data_vec[3]);
            const double y = std::stod(line_data_vec[4]);            
            const double alt = std::stod(line_data_vec[5]);
            int fix_state = std::stoi(line_data_vec[6]);
            int num_satellite = std::stoi(line_data_vec[7]);
            //LOG(INFO) << "timestamp : " << timestamp;

            //Eigen::Matrix3d cov = Eigen::Matrix3d::Zero();
            //for (size_t i = 0; i < 3; ++i) {
                //cov[i][i] = std::stod(line_data_vec[8+i]);
            //}

            double latitude_std = std::stod(line_data_vec[8]);
            double longitude_std = std::stod(line_data_vec[9]);
            double altitude_std = std::stod(line_data_vec[10]);
            
            //LOG(INFO) << "latitude_std : " << latitude_std;
            //LOG(INFO) << "longitude_std : " << longitude_std;
            //LOG(INFO) << "altitude_std : " << altitude_std;
   
            int heading_valid_flag = std::stoi(line_data_vec[12]);
            bool heading_valid = (heading_valid_flag == 1); 
            double heading = std::stod(line_data_vec[13]);
            
            //LOG(INFO) << "heading_valid : " << heading_valid;
            //LOG(INFO) << "heading : " << heading;
            //gnss_proc_(GNSS(time, 4, Vec3d(lat, lon, alt), heading, heading_valid, Vec3d(latitude_std, longitude_std, altitude_std)));
        } else if (sensor_type == "sick_middle") {
            if (time_sick_middle_map.find(time_str) == time_sick_middle_map.end()) {
                std::cout << "[main]: Failed to find sick_middle data at time: " << time_str << std::endl;
                continue;
            }

            const std::string& sick_middle_str = time_sick_middle_map.at(time_str);
            std::stringstream sick_middle_ss(sick_middle_str);
            line_data_vec.clear();
            while (std::getline(sick_middle_ss, value_str, ',')) { line_data_vec.push_back(value_str); }
            
            //LOG(INFO) << "sick_middle_str : " << sick_middle_str;
            //LOG(INFO) << "line_data_vec : " << line_data_vec[0];
            if(line_data_vec.empty()) {
                continue;
            }
            std::ifstream file;

            std::string frame_file = data_path + "sensor_data/SICK_middle/" + line_data_vec[0] + ".bin";
            //LOG(INFO) << "frame_file : " << frame_file;
            file.open(frame_file, std::ios::in | std::ios::binary);
            LaserScan scan;

            double degree_to_rad = 3.1415926536 / 180.0;
#if 1            
            scan.angle_min = -5.0 * degree_to_rad;
            scan.angle_max = 185.0 * degree_to_rad;
            scan.angle_increment = 0.666 * degree_to_rad;
#else 
            scan.angle_min = -1.65806281567;
            scan.angle_max = 1.65806281567;
            scan.angle_increment = 0.0116355288774;
#endif            
            scan.time_increment = 0.0;
            scan.scan_time = std::stod(line_data_vec[0]) * 0.000000001 - first_timestamp; // ns to s
            scan.range_min = 0.0;
            scan.range_max = 81.0;
            
            while (!file.eof()) {
                float range;
                float intensity;
                file.read(reinterpret_cast<char *>(&range), sizeof(float));
                file.read(reinterpret_cast<char *>(&intensity), sizeof(float));
                scan.ranges.push_back(range);
                scan.intensities.push_back((intensity));
            }

            //LOG(INFO) << "scan.scan_time : " << scan.scan_time;

            file.close();

            // add laser scan

            node.HandleLaserScanMessage(node_options.trajectory_id, sensor_type, scan);
  
            //LOG(INFO) << "timestamp : " << timestamp;
        } else if(sensor_type == "velodyne_left") {
            // if (time_velodyne_left_map.find(time_str) == time_velodyne_left_map.end()) {
            //     std::cout << "[main]: Failed to find sick_middle data at time: " << time_str << std::endl;
            //     continue;
            // }

            // const std::string& velodyne_left_str = time_velodyne_left_map.at(time_str);
            // std::stringstream velodyne_left_ss(velodyne_left_str);
            // line_data_vec.clear();
            // while (std::getline(velodyne_left_ss, value_str, ',')) { line_data_vec.push_back(value_str); }
            
            // //LOG(INFO) << "line_data_vec : " << line_data_vec[0];
            // const std::string velodyne_left_file = data_path + "sensor_data/VLP_left/" + line_data_vec[0] + ".bin"; 
                
            // std::ifstream file;
            // file.open(velodyne_left_file, std::ios::in | std::ios::binary);
            // if(!file.is_open()) {
            //     continue;
            // }

            // CloudPtr cloud_ptr;
            // cloud_ptr.reset(new pcl::PointCloud<pcl::PointXYZI>());
            // SimpleCloudPtr simple_cloud_ptr;
            // simple_cloud_ptr.reset(new pcl::PointCloud<SimplePointType>());

            // while (!file.eof()) {
            //     float x, y, z, intensity;
            //     file.read(reinterpret_cast<char *>(&x), sizeof(float));
            //     file.read(reinterpret_cast<char *>(&y), sizeof(float));
            //     file.read(reinterpret_cast<char *>(&z), sizeof(float));
            //     file.read(reinterpret_cast<char *>(&intensity), sizeof(float));

            //     //std::cout << " lidar point : " << x << " , " << y << " , " << z << " , " << intensity << std::endl;
            //     if(used_simple_point && first_frame == 0) {
            //         pcl::PointXYZI point_cloud;
            //         point_cloud.x = x;
            //         point_cloud.y = y;
            //         point_cloud.z = z;
            //         point_cloud.intensity = intensity;
            //         cloud_ptr->points.push_back(point_cloud);
            //     } else if(used_simple_point) {
            //         SimplePointType simple_point_cloud;
            //         simple_point_cloud.timestamp = timestamp;
            //         simple_point_cloud.x = x;
            //         simple_point_cloud.y = y;
            //         simple_point_cloud.z = z;
            //         simple_point_cloud.intensity = intensity;
            //         double dist = std::sqrt(x*x+y*y+z*z);
            //         double theta = asin(z/dist) * scale_;      
            //         int scan_id = (theta - min_theta_) / resolution_;
            //         if(scan_id >= num_scan_) {
            //             scan_id = num_scan_-1;
            //         }
            //         if(scan_id < 0) {
            //             scan_id = 0;
            //         }  

            //         //double angle = atan2(z, std::sqrt(x * x + y * y)) / 3.14159265 * 180 + 15;
            //         //int ring = round(angle / 2);
            //         //LOG(INFO) << "scan_id ======= : " << scan_id;
            //         //LOG(INFO) << "ring ======= : " << ring;

            //         simple_point_cloud.ring = scan_id;
            //         simple_cloud_ptr->points.push_back(simple_point_cloud);
            //     } else {
            //         pcl::PointXYZI point_cloud;
            //         point_cloud.x = x;
            //         point_cloud.y = y;
            //         point_cloud.z = z;
            //         point_cloud.intensity = intensity;
            //         cloud_ptr->points.push_back(point_cloud);    
            //     }
            // }

            // file.close();

            // TODO: add point cloud
            // if(!used_simple_point) {

            //     // cloud3d_proc_(cloud_ptr);
            // } else {
            //     if(first_frame == 0) {
            //         simple_cloud_ptr = TransSimpleCloud(cloud_ptr);
            //         first_frame++;
            //     }
            //     // simple_cloud_proc_(simple_cloud_ptr);                
            // }

        }  else if (sensor_type == "stereo") {
            // TODO: add image
            // const std::string img0_file = data_path + "image/stereo_left/" + time_str + ".png"; 
            // const std::string img1_file = data_path + "image/stereo_right/" + time_str + ".png"; 
            // cv::Mat im_left = cv::imread(img0_file);
            // cv::Mat im_right = cv::imread(img1_file);
            // cv::imshow("image", im_left);
            // cv::waitKey(5);
        }

        usleep(5000);
    }

    return 0;
}